﻿using System;
using System.Threading;
using System.Collections.Generic;
using System.Collections;
using System.Linq;
using System.Text;
using System.Diagnostics;
using MotionPlanning.MotionPlanningPrimitives;
using MotionPlanning.MotionPlanningComponents;
using MotionPlanning.MotionPlanningComponents.Visualizations;
using MotionPlanning.MotionPlanningComponents.MotionPlanningSystems;

namespace MotionPlanning.MotionPlanningAlgorithms.RDG_Prototype
{
    ///
    /// Roadmap with Diminishing Gaps
    ///
    public class RDG : Planner_RRTBased
    {
        protected double alpha;
        protected double epsilon;
        public int memMax;

        public override Node SpawnNode_Like(double[] q)
        {
            int DOF_state = this.System.Get_DOF_state();
            double[] q_new = new double[DOF_state];
            for (int i = 0; i < DOF_state; ++i)
                q_new[i] = q[i];
            Node x = Node.SpawnGraphicNode();
            x.set_Value(new NodeData_RDG(q_new, 0));
            return x;
        }
        public override Node SpawnNode_As(double[] q)
        {
            if (q.Length != this.System.Get_DOF_state())
                throw new Exception("q.length != DOF_state. in RDG.SpawnNode_As");
            Node x = Node.SpawnGraphicNode();
            x.set_Value(new NodeData_RDG(q, 0));
            return x;
        }
        public override Edge SpawnEdge()
        {
            Edge e = Edge.SpawnGraphicEdge();
            e.set_Value(new EdgeData_RDG());
            return e;
        }
        public override Edge SpawnEdge(Node entry, Node exit)
        {
            Edge e = Edge.SpawnGraphicEdge(entry, exit);
            e.set_Value(new EdgeData_RDG());
            return e;
        }
        public Edge SpawnEdge(Node entry, Node exit, double c_inc, double delta, double[] ctrl_inc)
        {
            Edge e = Edge.SpawnGraphicEdge(entry, exit);
            e.set_Value(new EdgeData_RDG(c_inc, delta, ctrl_inc));
            return e;
        }

        public RDG(RoboticSystemComponent system, VisualizationComponent visualizer, string motionPlanningConfigFile)
            : base(system, visualizer, motionPlanningConfigFile)
        {
            r_goal = -1;
            q_goal = new double[system.Get_DOF_state()];
            SolutionArray = new ArrayList();
            tmpDoubleArray1 = new ArrayList();
            tmpDoubleArray2 = new ArrayList();
            ///TODO read from file
            q_goal = new double[] { Math.PI / 2, 0 };
            r_goal = 0.5;
            epsilon = 0.1;
            ///
            memMax = 512;
        }

        public void SystemEvents_LowMemory(object sender, EventArgs e)
        {
            Debugger.Break();
        }

        // Execute RDG algorithm
        public override void Execute(int n_iterations)
        {
            if (x_root == null)
                throw new Exception("x_root uninitialized. Forget to call SetRoot()?");

            int iterations_extension = 0;
            int iterations_improvement = 0;
            while (iterations_extension < n_iterations)
            {
                ++iterations_total;

                long memUsed = GC.GetTotalMemory(false) / 1024 / 1024;
                if (memUsed > memMax)
                {
                    List<object> train = this.GetVertices().GetAllDataObjects();

                    for (int i = 0; i < train.Count; ++i)
                    {
                        this.CleanEdgesForEachNode((Node)train[i]);
                    }
                    
                    GC.Collect();
                    Thread.Sleep(4000);

                    long memCleaned = GC.GetTotalMemory(false) / 1024 / 1024;
                    if (memMax < memCleaned * 1.1)
                        memMax = (int)(memCleaned * 1.1);
                }
                
                #region Extension Phase

                bool DoExtension = true;
                if (iterations_extension > 100000)  DoExtension = false;
                if (DoExtension)
                {
                    ++iterations_extension;
                    /*
                    ----------------- Sampling Phase -------------------
                    */
                    double[] q_sample = this.Sampler.NextQ(SamplingComponent.SamplingMode.SAMPLING, this.System);
                    /*
                     ---------- Nearest Neighbor Search Phase -----------
                     */
                    double dist_nearest = double.MaxValue;
                    Object obj_nearest = null;
                    this.Vertices.NearestNeighbor_Approximate(q_sample, ref dist_nearest, ref obj_nearest);
                    if (obj_nearest == null) continue;
                    Node x_nearest = (Node)obj_nearest;
                    /*
                     ----------------- Extension Phase ------------------
                     */
                    //propagation
                    double[] u_inc = this.Sampler.NextControl(SamplingComponent.SamplingMode.CONTROL, this.System);
                    double t_inc = u_inc[u_inc.Length - 1];
                    double[] q_new = new double[this.System.Get_DOF_state()];
                    double cost_inc = 0;
                    bool succ_propagation = this.System.Propagate(x_nearest.get_Value().get_q(), u_inc, t_inc, ref q_new, ref cost_inc);
                    if (succ_propagation == false) continue;
                    bool inRange = this.System.PointInRange(q_new);
                    if (inRange == false) continue;
                    //grow graph
                    Node x_new = this.SpawnNode_As(q_new);
                    this.Vertices.AddPoint(x_new.get_Value().get_q(), x_new);

                    Edge e_new = this.SpawnEdge(x_nearest, x_new, cost_inc, 0, u_inc);

                    NodeData_RDG v_new = (NodeData_RDG)x_new.get_Value();
                    v_new.set_utility_c(x_nearest.get_Value().get_utility_c() + CalculateUtility(e_new));

                    x_nearest.add_ChildEdge(e_new);
                    x_new.add_ParentEdge(e_new);

                    bool inGoal = this.GoalReached(q_new);
                    if (inGoal == true)
                    {
                        double cost_RRT = GetRRTSolutionCost(x_new);
                        Trajectory t = new Trajectory(x_new);
                    }
                }

                #endregion

                #region Improve Phase
                
                bool DoImprovement = true;
                if (DoImprovement)
                {
                    ++iterations_improvement;
                    /*
                     ----------------- Sampling Phase -------------------
                     */
                    double[] q_sample = this.Sampler.NextQ(SamplingComponent.SamplingMode.SELECT, this.System);
                    /*
                     ---------- Nearest Neighbor Search Phase -----------
                     */
                    double dist_nearest = double.MaxValue;
                    Object obj_nearest = null;
                    this.Vertices.NearestNeighbor_Approximate(q_sample, ref dist_nearest, ref obj_nearest);
                    if (obj_nearest == null) continue;
                    Node x_nearest = (Node)obj_nearest;
                    /*
                     ----------------- Improvement Phase -----------------
                     */
                    int n_try = 100;
                    alpha = 8; /// TODO increase alpha accordingly
                               /// was 3 for point2D
                    double[] q_iTry = new double[this.System.Get_DOF_state()];
                    for (int i = 0; i < n_try; ++i)
                    {
                        //shooting
                        double[] u_inc = this.Sampler.NextControl(SamplingComponent.SamplingMode.SHOOTING, this.System);
                        //double t_inc = u_inc[u_inc.Length - 1] * 10;
                        double t_inc = u_inc[u_inc.Length - 1];
                        double cost_inc = 0;
                        bool succ_propagation = this.System.Propagate_HighSpeed(x_nearest.get_Value().get_q(), u_inc, t_inc, ref q_iTry, ref cost_inc);
                        if (succ_propagation == false) continue;
                        bool inRange = this.System.PointInRange(q_iTry);
                        if (inRange == false) continue;

                        //shoot matching
                        double iTryDist = double.MaxValue;
                        Object obj_iNear = null;
                        this.Vertices.NearestNeighbor_Approximate(q_iTry, ref iTryDist, ref obj_iNear);
                        if (obj_iNear == null) continue;

                        //find c_iNear
                        Node x_iNear = (Node)obj_iNear;
                        double c_iNear = x_iNear.get_Value().get_utility_c();

                        //find c_iTry
                        double delta = Math.Sqrt(this.System.Distance2_Topological(q_iTry, x_iNear.get_Value().get_q()));
                        double utility_inc = RDG.CalculateUtility(cost_inc, delta, this.epsilon, this.alpha);
                        double c_iTry = x_nearest.get_Value().get_utility_c() + utility_inc;

                        //TODO debug
                        //UtilValidation(x_nearest);

                        //rewiring
                        List<Edge> parents_iNear = x_iNear.get_Parents();
                        if (c_iTry < c_iNear)
                        {
                            Edge e = this.SpawnEdge(x_nearest, x_iNear, cost_inc, delta, u_inc);
                            x_nearest.add_ChildEdge(e);
                            parents_iNear.Add(e);
                            x_iNear.get_Value().set_utility_c(c_iTry);

                            double v1 = x_iNear.get_Value().get_utility_c() - x_nearest.get_Value().get_utility_c();
                            double v2 = this.CalculateUtility(x_iNear.get_Parents()[x_iNear.get_Parents().Count - 1]);
                            if (Math.Abs(v1 - v2) > 0.0000001)
                                Debugger.Break();



                            this.level = 0;
                            InfluenceBranch(x_iNear);
                        }
                        else if (parents_iNear != null)
                        {
                            for (int j = 0; j < parents_iNear.Count; ++j)
                            {
                                Edge jEdge = parents_iNear[j];
                                double delta_jEdge = ((EdgeData_RDG)jEdge.get_Value()).get_utility_inc_delta();
                                if (delta < delta_jEdge)
                                {
                                    Edge e = this.SpawnEdge(x_nearest, x_iNear, cost_inc, delta, u_inc);
                                    x_nearest.add_ChildEdge(e);
                                    //the last one in the array is the best edge
                                    //put the new edge at the second last position
                                    Edge e_best = parents_iNear[parents_iNear.Count - 1];
                                    parents_iNear.Add(e_best);
                                    parents_iNear[parents_iNear.Count - 2] = e;
                                    break;
                                }
                            }
                        }

                        //TODO debug
                        //UtilValidation(x_iNear);
                    }
                }

                #endregion
            }
        }

        //TODO delete this
        public int n_reached = 0;
        public int n_valid = 0;
        public int level = 0;
        public ArrayList SolutionArray;
        public ArrayList tmpDoubleArray1;
        public ArrayList tmpDoubleArray2;



        public List<object> SimulateTrajectory_Vibration(Trajectory solution, ref double[] q_end, int n_max, double duration_noise, double amplitude_noise)
        {
            List<Edge> edges = solution.GetEdges();
            List<Node> nodes = solution.GetNodes();
            int DOF = this.System.Get_DOF_state();
            double[] q_entry = new double[DOF];
            double[] q_exit = new double[DOF];
            double[] q_simu = new double[DOF];
            MotionPlanningUtils.vector_set(nodes[0].get_Value().get_q(),ref q_exit);
            double solution_cost = 0;
            List<object> nodeArray = new List<object>();
            nodeArray.Add(this.SpawnNode_Like(nodes[0].get_Value().get_q()));
            double[] ctrl_noise = new double[this.System.Get_DOF_control() + 1];
            double[] ctrl_sum = new double[ctrl_noise.Length];
            
            for (int i = 0; i < edges.Count - 1; ++i)
            {
                MotionPlanningUtils.vector_set(q_exit, ref q_entry);
                Edge e_cur = edges[i];
                Node x_cur = nodes[i + 1];
                double[] ctrl_inc = e_cur.get_Value().get_ctrl_inc();

                double minDistance2=double.MaxValue;
                double[] ctrl_best = new double[ctrl_sum.Length];
                double cost_inc_best = 0;
                for (int j = 0; j < n_max; ++j)
                {
                    this.Sampler.NextNoisyControl(this.System, amplitude_noise, duration_noise, ref ctrl_noise);
                    MotionPlanningUtils.vector_add(ctrl_inc, ctrl_noise, ref ctrl_sum);
                    if(ctrl_sum[ctrl_sum.Length-1]<0)
                        ctrl_sum[ctrl_sum.Length - 1] = ctrl_inc[ctrl_sum.Length - 1];
                    double cost_inc_noise = 0;
                    this.System.Propagate_HighSpeed(q_entry,ctrl_sum,ctrl_sum[ctrl_sum.Length-1],ref q_simu,ref cost_inc_noise);
                    double cur_dist2 = this.System.Distance2_Topological(q_simu,x_cur.get_Value().get_q());
                    if (cur_dist2 < minDistance2)
                    {
                        minDistance2 = cur_dist2;
                        MotionPlanningUtils.vector_set(ctrl_sum, ref ctrl_best);
                        MotionPlanningUtils.vector_set(q_simu, ref q_exit);
                        cost_inc_best = cost_inc_noise;
                    }
                }
                Node curNode = this.SpawnNode_Like(q_exit);
                solution_cost += cost_inc_best;
                curNode.get_Value().set_utility_c(solution_cost);
                curNode.add_ParentEdge(this.SpawnEdge((Node)nodeArray[nodeArray.Count - 1], curNode));
                q_end = curNode.get_Value().get_q();
                nodeArray.Add(curNode);
            }
            return nodeArray;
        }

        public List<object> SimulateTrajectory(Trajectory solution, ref Node x_end)
        {
            List<Edge> edges = solution.GetEdges();
            List<Node> nodes = solution.GetNodes();
            x_end = this.SpawnNode_As(new double[nodes[0].get_Value().get_q().Length]);
            double[] q_tmp = new double[x_end.get_Value().get_q().Length];

            NodeData_RDG v_end = ((NodeData_RDG)x_end.get_Value());
            v_end.set_q(nodes[0].get_Value().get_q());
            v_end.set_utility_c(0);
            double solution_cost = 0;
            List<object> nodeArray = new List<object>();
            nodeArray.Add(this.SpawnNode_Like(nodes[0].get_Value().get_q()));

            for (int i = 0; i < edges.Count - 1; ++i)
            {
                Edge e_cur = edges[i];
                double[] ctrl_inc = e_cur.get_Value().get_ctrl_inc();
                double cost_inc = 0;
                this.System.Propagate(v_end.get_q(), ctrl_inc, ctrl_inc[ctrl_inc.Length - 1], ref q_tmp, ref cost_inc);
                solution_cost += cost_inc;
                v_end.set_q(q_tmp);
                v_end.set_utility_c(solution_cost);

                Node curNode = this.SpawnNode_Like(q_tmp);
                curNode.add_ParentEdge(this.SpawnEdge((Node)nodeArray[nodeArray.Count - 1], curNode));
                nodeArray.Add(curNode);
            }
            return nodeArray;
        }

        // recursive function to propagate the utility changes among a sub-branch
        private void InfluenceBranch(Node x_iNear)
        {
            //TODO debug
            ++level;
            if (level > 10000000)
            {
                Debugger.Break();
            }

            NodeData_RDG v_entry = (NodeData_RDG)x_iNear.get_Value();
            double u_root = v_entry.get_utility_c();
            double[] q_entry = v_entry.get_q();
            if (GoalReached(q_entry, 1.25))//was 2
            {
                ///TODO test code
                //Debugger.Break();
                ++n_reached;

                Trajectory solution = new Trajectory(x_iNear);
                double[] q_end = null;
                //this.SimulateTrajectory(solution,ref x_end);
                List<object> realPath = this.SimulateTrajectory_Vibration(solution,ref q_end,50,0.2,0.1);
                Node x_end_path = (Node)realPath[realPath.Count - 1];
                bool IsValid = this.GoalReached(q_end);
                //IsValid = true;//rm
                if (IsValid == true)
                {
                    ++n_valid;

                    if (this.solution != null)
                    {
                        double best = this.solution.GetCost();
                        if (x_end_path.get_Value().get_utility_c() < best)
                        {
                            this.solution = solution;
                            this.solution.SetCost(x_end_path.get_Value().get_utility_c());
                            //this.solution.SetCost(1000); //remove this
                        }
                    }
                    else
                    {
                        this.solution = solution;
                        this.solution.SetCost(x_end_path.get_Value().get_utility_c());
                    }


                    tmpDoubleArray1.Add(x_end_path.get_Value().get_utility_c()); //real gap-free cost
                    tmpDoubleArray2.Add(v_entry.get_utility_c());           //utility of this alpha-variant solution

                    //if (v_entry.get_utility_c() < x_end_path.get_Value().get_utility_c())
                        //Debugger.Break();

                }
                return;
            }
            double u_iNear = x_iNear.get_Value().get_utility_c();
            List<Edge> children = x_iNear.get_Children();
            if (children == null)   return;

            for (int i = 0; i < children.Count; ++i)
            {
                Edge e_child = children[i];
                Node x_child = e_child.get_exit();
                List<Edge> parents_x_child = x_child.get_Parents();
                double u_iInfulence = u_iNear + this.CalculateUtility(e_child);
                double u_iCurrent = x_child.get_Value().get_utility_c();

                if (parents_x_child.Count <= 0) continue;
                Edge e_realParent = parents_x_child[parents_x_child.Count - 1];

                if (u_iInfulence < u_iCurrent)
                {
                    x_child.get_Value().set_utility_c(u_iInfulence);
                    if (e_child != e_realParent)
                    {
                        //a candidate parent is becoming the current parent
                        int newParentIdx = parents_x_child.IndexOf(e_child);
                        Edge e_tmp = parents_x_child[newParentIdx];
                        parents_x_child[newParentIdx] = e_realParent;
                        parents_x_child[parents_x_child.Count - 1] = e_tmp;
                    }

                    List<Edge> E_parents = x_child.get_Parents();
                    Edge latestParent = E_parents[E_parents.Count - 1];
                    Node parent = latestParent.get_entry();

                    double v1 = x_child.get_Value().get_utility_c() - parent.get_Value().get_utility_c();
                    double v2 = this.CalculateUtility(latestParent);
                    if (Math.Abs(v1 - v2) > 0.0000001)
                        Debugger.Break();



                    InfluenceBranch(x_child);
                }
                else
                {
                    if (e_child == e_realParent)
                        Debugger.Break();
                }
            }
        }
        
        // test if q_test is in the goal region
        public override bool GoalReached(double[] q_test)
        {
            if(r_goal<=0)
                throw new NotImplementedException("Forget to call SetGoal and SetGoalTolerance first?");
            double distance2 = this.System.Distance2_Euclidean(q_goal, q_test);
            if (distance2 <= r_goal * r_goal)
                return true;
            else
                return false;
        }

        public bool GoalReached(double[] q_test, double n_times)
        {
            if (r_goal <= 0)
                throw new NotImplementedException("Forget to call SetGoal and SetGoalTolerance first?");
            double distance2 = this.System.Distance2_Euclidean(q_goal, q_test);
            if (distance2 <= r_goal * r_goal * n_times * n_times)
                return true;
            else
                return false;
        }

        public double GetRRTSolutionCost(Node x_reach)
        {
            double cost=0;
            Node x_current = x_reach;
            List<Edge> E_reach = x_current.get_Parents();
            while (E_reach != null)
            {
                Edge e = null;
                for (int i = 0; i < E_reach.Count; ++i)
                {
                    e = E_reach[i];
                    if (((EdgeData_RDG)e.get_Value()).get_utility_inc_delta() <= 0)
                        break;
                }
                x_current = e.get_entry();
                E_reach = x_current.get_Parents();
                cost += e.get_Value().get_utility_inc_c();
            }
            return cost;
        }

        public double GetGoalTolerance()
        {
            return this.r_goal;
        }

        public void SetGoalTolerance(double radius)
        {
            this.r_goal = radius;
        }

        override public void SetGoal(double[] q_goal)
        {
            this.q_goal = q_goal;
        }

        public double[] GetGoal()
        {
            return q_goal;
        }

        public override string ToString()
        {
            return "\n\t<Algorithm_Type>Standard_RDG</Algorithm_Type>" +
                    "\n\t<Algorithm_Parameters>"+
                    "\n\t\t<Goal_State>" + MotionPlanningUtils.ArrayToString(q_goal) + "</Goal_State>" +
                    "\n\t\t<Goal_Radius>" + this.r_goal + "</Goal_Radius>" +
                    "\n\t</Algorithm_Parameters>" + "\n";
        }

        // the evaluation function for score gaps
        public double pFunction(double delta)
        {
            double p = Math.Exp(-delta);
            return p;
        }

        public static double CalculateUtility(double c, double delta, double epsilon, double alpha)
        {
            if (delta < 0)
                throw new Exception("delta must be positive. epsilon=" + delta);
            if (c <= 0)
                throw new Exception("c must be positive. epsilon=" + c);
            
            //double utility = c + epsilon * Math.Exp( delta * alpha );
            double utility = c * Math.Exp(delta * alpha);

            if (utility < c)
                Debugger.Break();

            return utility;
        }

        public double CalculateUtility(Edge e)
        {
            EdgeData_RDG edgeData = (EdgeData_RDG)e.get_Value();
            return RDG.CalculateUtility(edgeData.get_utility_inc_c(), edgeData.get_utility_inc_delta(), this.epsilon,this.alpha);
        }

        public override void Test()
        {
            throw new NotImplementedException();
        }

        public override void Plot()
        {
            List<object> graph = Vertices.GetAllDataObjects();
            Visualizer.PlotEdges(graph);
        }

        public bool UtilValidation(Node son)
        {
            if (son == null) return true;
            List<Edge> Es = son.get_Parents();
            if (Es == null) return true;

            double u_last = ((NodeData_RDG)son.get_Value()).get_utility_c();
            int step = 0;
            while (Es != null)
            {
                step++;
                Node x_trace = Es[Es.Count - 1].get_entry();
                double u_trace = x_trace.get_Value().get_utility_c();
                if (u_trace > u_last)
                {
                    Debugger.Break();
                    return false;
                }
                u_last = u_trace;
                Es = x_trace.get_Parents();
            }

            return true;
        }

        public void CleanEdgesForEachNode(Node x_i)
        {
            List<Edge> X_parents = x_i.get_Parents();
            if (X_parents == null) return;
            if (X_parents.Count == 0) return;
            double delta_threshold = ((EdgeData_RDG)X_parents[X_parents.Count - 1].get_Value()).get_utility_inc_delta();
            List<Edge> X_parents_new = new List<Edge>();
            for (int i = 0; i < X_parents.Count - 1; ++i)
            {
                Edge e_i = X_parents[i];
                double delta_i = ((EdgeData_RDG)e_i.get_Value()).get_utility_inc_delta();
                if (delta_i < delta_threshold)
                {
                    X_parents_new.Add(e_i);
                }
                else
                {
                    e_i.get_entry().get_Children().Remove(e_i);
                }
            }
            X_parents_new.Add(X_parents[X_parents.Count - 1]);
            x_i.set_Parents(X_parents_new);
        }
    }
}
